
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mb_motors_class.h
  * @author     baiyang
  * @date       2023-1-29
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdbool.h>
#include <stdint.h>
#include <stddef.h>

#include <rtthread.h>

#include <common/gp_defines.h>
#include <common/filter/lpfilter.h>
#include <common/gp_math/gp_mathlib.h>
#include <common/filter/derivative_filter.h>
#include <gcs_mavlink/gcs_mavlink.h>
/*-----------------------------------macro------------------------------------*/
// offsets for motors in motor_out and _motor_filtered arrays
#define MB_MOTORS_MOT_1 0U
#define MB_MOTORS_MOT_2 1U
#define MB_MOTORS_MOT_3 2U
#define MB_MOTORS_MOT_4 3U
#define MB_MOTORS_MOT_5 4U
#define MB_MOTORS_MOT_6 5U
#define MB_MOTORS_MOT_7 6U
#define MB_MOTORS_MOT_8 7U
#define MB_MOTORS_MOT_9 8U
#define MB_MOTORS_MOT_10 9U
#define MB_MOTORS_MOT_11 10U
#define MB_MOTORS_MOT_12 11U

#define MB_MOTORS_MAX_NUM_MOTORS 12

#ifndef MB_MOTORS_FRAME_DEFAULT_ENABLED
#define MB_MOTORS_FRAME_DEFAULT_ENABLED 1
#endif

#ifndef MB_MOTORS_FRAME_QUAD_ENABLED
#define MB_MOTORS_FRAME_QUAD_ENABLED MB_MOTORS_FRAME_DEFAULT_ENABLED
#endif
#ifndef MB_MOTORS_FRAME_HEXA_ENABLED
#define MB_MOTORS_FRAME_HEXA_ENABLED MB_MOTORS_FRAME_DEFAULT_ENABLED
#endif
#ifndef MB_MOTORS_FRAME_OCTA_ENABLED
#define MB_MOTORS_FRAME_OCTA_ENABLED MB_MOTORS_FRAME_DEFAULT_ENABLED
#endif
#ifndef MB_MOTORS_FRAME_DECA_ENABLED
#define MB_MOTORS_FRAME_DECA_ENABLED MB_MOTORS_FRAME_DEFAULT_ENABLED
#endif
#ifndef MB_MOTORS_FRAME_DODECAHEXA_ENABLED
#define MB_MOTORS_FRAME_DODECAHEXA_ENABLED MB_MOTORS_FRAME_DEFAULT_ENABLED
#endif
#ifndef MB_MOTORS_FRAME_Y6_ENABLED
#define MB_MOTORS_FRAME_Y6_ENABLED MB_MOTORS_FRAME_DEFAULT_ENABLED
#endif
#ifndef MB_MOTORS_FRAME_OCTAQUAD_ENABLED
#define MB_MOTORS_FRAME_OCTAQUAD_ENABLED MB_MOTORS_FRAME_DEFAULT_ENABLED
#endif

// motor update rate
#define MB_MOTORS_SPEED_DEFAULT     490 // default output rate to the motors
/*----------------------------------typedef-----------------------------------*/
enum mb_motor_frame_class {
    MB_MOTOR_FRAME_UNDEFINED = 0,
    MB_MOTOR_FRAME_QUAD = 1,
    MB_MOTOR_FRAME_HEXA = 2,
    MB_MOTOR_FRAME_OCTA = 3,
    MB_MOTOR_FRAME_OCTAQUAD = 4,
    MB_MOTOR_FRAME_Y6 = 5,
    MB_MOTOR_FRAME_HELI = 6,
    MB_MOTOR_FRAME_TRI = 7,
    MB_MOTOR_FRAME_SINGLE = 8,
    MB_MOTOR_FRAME_COAX = 9,
    MB_MOTOR_FRAME_TAILSITTER = 10,
    MB_MOTOR_FRAME_HELI_DUAL = 11,
    MB_MOTOR_FRAME_DODECAHEXA = 12,
    MB_MOTOR_FRAME_HELI_QUAD = 13,
    MB_MOTOR_FRAME_DECA = 14,
    MB_MOTOR_FRAME_SCRIPTING_MATRIX = 15,
    MB_MOTOR_FRAME_6DOF_SCRIPTING = 16,
    MB_MOTOR_FRAME_DYNAMIC_SCRIPTING_MATRIX = 17,
};

enum mb_motor_frame_type {
    MB_MOTOR_FRAME_TYPE_PLUS = 0,
    MB_MOTOR_FRAME_TYPE_X = 1,
    MB_MOTOR_FRAME_TYPE_V = 2,
    MB_MOTOR_FRAME_TYPE_H = 3,
    MB_MOTOR_FRAME_TYPE_VTAIL = 4,
    MB_MOTOR_FRAME_TYPE_ATAIL = 5,
    MB_MOTOR_FRAME_TYPE_PLUSREV = 6, // plus with reversed motor direction
    MB_MOTOR_FRAME_TYPE_Y6B = 10,
    MB_MOTOR_FRAME_TYPE_Y6F = 11, // for FireFlyY6
    MB_MOTOR_FRAME_TYPE_BF_X = 12, // X frame, betaflight ordering
    MB_MOTOR_FRAME_TYPE_DJI_X = 13, // X frame, DJI ordering
    MB_MOTOR_FRAME_TYPE_CW_X = 14, // X frame, clockwise ordering
    MB_MOTOR_FRAME_TYPE_I = 15, // (sideways H) octo only
    MB_MOTOR_FRAME_TYPE_NYT_PLUS = 16, // plus frame, no differential torque for yaw
    MB_MOTOR_FRAME_TYPE_NYT_X = 17, // X frame, no differential torque for yaw
    MB_MOTOR_FRAME_TYPE_BF_X_REV = 18, // X frame, betaflight ordering, reversed motors
    MB_MOTOR_FRAME_TYPE_Y4 = 19, //Y4 Quadrotor frame
};

// desired spool states
enum MotorDesiredSpoolState {
    MB_MOTOR_DESIRED_SHUT_DOWN = 0,              // all motors should move to stop
    MB_MOTOR_DESIRED_GROUND_IDLE = 1,            // all motors should move to ground idle
    MB_MOTOR_DESIRED_THROTTLE_UNLIMITED = 2,     // motors should move to being a state where throttle is unconstrained (e.g. by start up procedure)
};

// spool states
enum MotorSpoolState {
    MB_MOTOR_SHUT_DOWN = 0,                      // all motors stop
    MB_MOTOR_GROUND_IDLE = 1,                    // all motors at ground idle
    MB_MOTOR_SPOOLING_UP = 2,                       // increasing maximum throttle while stabilizing
    MB_MOTOR_THROTTLE_UNLIMITED = 3,             // throttle is no longer constrained by start up procedure
    MB_MOTOR_SPOOLING_DOWN = 4,                     // decreasing maximum throttle while stabilizing
};

enum mb_motor_pwm_type {
    MB_MOTOR_PWM_TYPE_NORMAL     = 0,
    MB_MOTOR_PWM_TYPE_ONESHOT    = 1,
    MB_MOTOR_PWM_TYPE_ONESHOT125 = 2,
    MB_MOTOR_PWM_TYPE_BRUSHED    = 3,
    MB_MOTOR_PWM_TYPE_DSHOT150   = 4,
    MB_MOTOR_PWM_TYPE_DSHOT300   = 5,
    MB_MOTOR_PWM_TYPE_DSHOT600   = 6,
    MB_MOTOR_PWM_TYPE_DSHOT1200  = 7,
    MB_MOTOR_PWM_TYPE_PWM_RANGE  = 8,
};

typedef struct mb_motors* mb_motors_t;
typedef const struct mb_motors* mb_motors_const_t;
typedef struct mb_motors_ops* mb_motors_ops_t;

struct mb_motors {
    mb_motors_ops_t     ops;

    // structure for holding motor limit flags
    struct mb_motors_limit {
        uint8_t roll            : 1; // we have reached roll or pitch limit
        uint8_t pitch           : 1; // we have reached roll or pitch limit
        uint8_t yaw             : 1; // we have reached yaw limit
        uint8_t throttle_lower  : 1; // we have reached throttle's lower limit
        uint8_t throttle_upper  : 1; // we have reached throttle's upper limit
    } limit;

    // internal variables
    float               _dt;                        // time difference (in seconds) since the last loop time
    uint16_t            _speed_hz;                  // speed in hz to send updates to motors
    float               _roll_in;                   // desired roll control from attitude controllers, -1 ~ +1
    float               _roll_in_ff;                // desired roll feed forward control from attitude controllers, -1 ~ +1
    float               _pitch_in;                  // desired pitch control from attitude controller, -1 ~ +1
    float               _pitch_in_ff;               // desired pitch feed forward control from attitude controller, -1 ~ +1
    float               _yaw_in;                    // desired yaw control from attitude controller, -1 ~ +1
    float               _yaw_in_ff;                 // desired yaw feed forward control from attitude controller, -1 ~ +1
    float               _throttle_in;               // last throttle input from set_throttle caller
    float               _throttle_out;              // throttle after mixing is complete
    float               _throttle_slew_rate;        // throttle slew rate from input
    float               _forward_in;                // last forward input from set_forward caller
    float               _lateral_in;                // last lateral input from set_lateral caller
    float               _throttle_avg_max;          // last throttle input from set_throttle_avg_max
    LowPassFiltFloat    _throttle_filter;           // pilot throttle input filter
    DerivativeFilterFloat_Size7  _throttle_slew;    // throttle output slew detector
    LowPassFiltFloat    _throttle_slew_filter;      // filter for the output of the throttle slew
    enum MotorDesiredSpoolState   _spool_desired;             // desired spool state
    enum MotorSpoolState          _spool_state;               // current spool mode

    // air pressure compensation variables
    float               _air_density_ratio;     // air density / sea level density - decreases in altitude

    // mask of what channels need fast output
    uint32_t            _motor_fast_mask;

    // mask of what channels need to use SERVOn_MIN/MAX for output mapping
    uint32_t            _motor_pwm_range_mask;
    
    // pass through variables
    float _roll_radio_passthrough;     // roll input from pilot in -1 ~ +1 range.  used for setup and providing servo feedback while landed
    float _pitch_radio_passthrough;    // pitch input from pilot in -1 ~ +1 range.  used for setup and providing servo feedback while landed
    float _throttle_radio_passthrough; // throttle/collective input from pilot in 0 ~ 1 range.  used for setup and providing servo feedback while landed
    float _yaw_radio_passthrough;      // yaw input from pilot in -1 ~ +1 range.  used for setup and providing servo feedback while landed

    Param_int8          _pwm_type;            // PWM output type

    // motor failure handling
    bool                _thrust_boost;          // true if thrust boost is enabled to handle motor failure
    bool                _thrust_balanced;       // true when output thrust is well balanced
    float               _thrust_boost_ratio;    // choice between highest and second highest motor output for output mixing (0 ~ 1). Zero is normal operation

    MAV_TYPE            _mav_type; // MAV_TYPE_GENERIC = 0;

    bool                _armed;             // 0 if disarmed, 1 if armed
    bool                _interlock;         // 1 if the motor interlock is enabled (i.e. motors run), 0 if disabled (motors don't run)
    bool                _initialised_ok;    // 1 if initialisation was successful
    bool                _spoolup_block;     // true if spoolup is blocked
};

struct mb_motors_ops {
    // check initialisation succeeded
    bool        (*arming_checks)(mb_motors_const_t motors, size_t buflen, char *buffer);

    // for 6DoF vehicles, sets the roll and pitch offset, this rotates the thrust vector in body frame
    void        (*set_roll_pitch)(mb_motors_t motors, float roll_deg, float pitch_deg);

    float       (*get_throttle_hover)(mb_motors_const_t motors);

    uint8_t     (*get_lost_motor)(mb_motors_const_t motors);

    // set update rate to motors - a value in hertz
    void        (*set_update_rate)(mb_motors_t motors, uint16_t speed_hz);

    // init
    void        (*init)(mb_motors_t motors, enum mb_motor_frame_class frame_class, enum mb_motor_frame_type frame_type);

    // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
    void        (*set_frame_class_and_type)(mb_motors_t motors, enum mb_motor_frame_class frame_class, enum mb_motor_frame_type frame_type);

    // output - sends commands to the motors
    void        (*output)(mb_motors_t motors);

    // output_min - sends minimum values out to the motors
    void        (*output_min)(mb_motors_t motors);

    // get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
    //  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
    uint32_t    (*get_motor_mask)(mb_motors_t motors);

    // return the roll factor of any motor, this is used for tilt rotors and tail sitters
    // using copter motors for forward flight
    float       (*get_roll_factor)(mb_motors_t motors, uint8_t i);

    // return the pitch factor of any motor
    float       (*get_pitch_factor)(mb_motors_t motors, uint8_t i);

    // return whether a motor is enabled or not
    bool        (*is_motor_enabled)(mb_motors_t motors, uint8_t i);

    // This function required for tradheli. Tradheli initializes targets when going from unarmed to armed state.
    // This function is overriden in motors_heli class.   Always true for multicopters.
    bool        (*init_targets_on_arming)(mb_motors_const_t motors);

    // direct motor write
    void        (*rc_write)(mb_motors_t motors, uint8_t chan, uint16_t pwm);

    // write log, to be called at 10hz
    void        (*Log_Write)(mb_motors_t motors);

    // output functions that should be overloaded by child classes
    void        (*output_armed_stabilizing)(mb_motors_t motors);
    void        (*rc_write_angle)(mb_motors_t motors, uint8_t chan, int16_t angle_cd);
    void        (*rc_set_freq)(mb_motors_t motors, uint32_t mask, uint16_t freq_hz);

    // update the throttle input filter
    void        (*update_throttle_filter)(mb_motors_t motors);

    // save parameters as part of disarming
    void        (*save_params_on_disarm)(mb_motors_t motors);

    // return string corresponding to frame_class
    const char* (*_get_frame_string)(mb_motors_const_t motors);

    // return string corresponding to frame_type
    const char* (*get_type_string)(mb_motors_const_t motors);

    // output_test_seq - spin a motor at the pwm value specified
    //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
    //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
    void        (*_output_test_seq)(mb_motors_t motors, uint8_t motor_seq, int16_t pwm);

    // set the value of motor module parameters
    void        (*assign_param)(mb_motors_t motors);
};

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void                mb_motors_ctor(mb_motors_t motors, mb_motors_ops_t ops, uint16_t speed_hz);

// returns a formatted string into buffer, e.g. "QUAD/X"
void                mb_motors_get_frame_and_type_string(mb_motors_const_t motors, char *buffer, uint8_t buflen);

/*
  map an internal motor mask to real motor mask, accounting for
  SERVOn_FUNCTION mappings, and allowing for multiple outputs per
  motor number
 */
uint32_t            mb_motors_motor_mask_to_srv_channel_mask(mb_motors_const_t motors, uint32_t mask);

// output_test_seq - spin a motor at the pwm value specified
//  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void                mb_motors_output_test_seq(mb_motors_t motors, uint8_t motor_seq, int16_t pwm);

// set limit flag for pitch, roll and yaw
void                mb_motors_set_limit_flag_pitch_roll_yaw(mb_motors_t motors, bool flag);

// pilot input in the -1 ~ +1 range for roll, pitch and yaw. 0~1 range for throttle
void                mb_motors_set_radio_passthrough(mb_motors_t motors, float roll_input, float pitch_input, float throttle_input, float yaw_input);

// add a motor to the motor map
void                mb_motors_add_motor_num(mb_motors_t motors, int8_t motor_num);

static inline bool  mb_motors_initialised_ok(mb_motors_const_t motors) { return motors->_initialised_ok; }
static inline void  mb_motors_set_initialised_ok(mb_motors_t motors, bool val) { motors->_initialised_ok = val; }

// arm, disarm or check status status of motors
static inline bool  mb_motors_armed(mb_motors_const_t motors) { return motors->_armed; }
void                mb_motors_armed2(mb_motors_t motors, bool arm);

// set motor interlock status
static inline void  mb_motors_set_interlock(mb_motors_t motors, bool set) { motors->_interlock = set;}

// get motor interlock status.  true means motors run, false motors don't run
static inline bool  mb_motors_get_interlock(mb_motors_const_t motors) { return motors->_interlock; }

// get/set spoolup block
static inline bool  mb_motors_get_spoolup_block(mb_motors_const_t motors) { return motors->_spoolup_block; }
static inline void  mb_motors_set_spoolup_block(mb_motors_t motors, bool set) { motors->_spoolup_block = set; }

// set_roll, set_pitch, set_yaw, set_throttle
static inline void  mb_motors_set_roll(mb_motors_t motors, float roll_in) { motors->_roll_in = roll_in; };        // range -1 ~ +1
static inline void  mb_motors_set_roll_ff(mb_motors_t motors, float roll_in) { motors->_roll_in_ff = roll_in; };    // range -1 ~ +1
static inline void  mb_motors_set_pitch(mb_motors_t motors, float pitch_in) { motors->_pitch_in = pitch_in; };    // range -1 ~ +1
static inline void  mb_motors_set_pitch_ff(mb_motors_t motors, float pitch_in) { motors->_pitch_in_ff = pitch_in; };  // range -1 ~ +1
static inline void  mb_motors_set_yaw(mb_motors_t motors, float yaw_in) { motors->_yaw_in = yaw_in; };            // range -1 ~ +1
static inline void  mb_motors_set_yaw_ff(mb_motors_t motors, float yaw_in) { motors->_yaw_in_ff = yaw_in; };      // range -1 ~ +1
static inline void  mb_motors_set_throttle(mb_motors_t motors, float throttle_in) { motors->_throttle_in = throttle_in; };   // range 0 ~ 1
static inline void  mb_motors_set_throttle_avg_max(mb_motors_t motors, float throttle_avg_max) { motors->_throttle_avg_max = math_constrain_float(throttle_avg_max, 0.0f, 1.0f); };   // range 0 ~ 1
static inline void  mb_motors_set_throttle_filter_cutoff(mb_motors_t motors, float filt_hz) { lpf_set_cutoff1(&motors->_throttle_filter, filt_hz); }
static inline void  mb_motors_set_slew_filter_cutoff(mb_motors_t motors, float filt_hz) { lpf_set_cutoff1(&motors->_throttle_slew_filter, filt_hz); }
static inline void  mb_motors_set_forward(mb_motors_t motors, float forward_in) { motors->_forward_in = forward_in; }; // range -1 ~ +1
static inline void  mb_motors_set_lateral(mb_motors_t motors, float lateral_in) { motors->_lateral_in = lateral_in; };     // range -1 ~ +1

// accessors for roll, pitch, yaw and throttle inputs to motors
static inline float mb_motors_get_roll(mb_motors_const_t motors) { return motors->_roll_in; }
static inline float mb_motors_get_roll_ff(mb_motors_const_t motors) { return motors->_roll_in_ff; }
static inline float mb_motors_get_pitch(mb_motors_const_t motors) { return motors->_pitch_in; }
static inline float mb_motors_get_pitch_ff(mb_motors_const_t motors) { return motors->_pitch_in_ff; }
static inline float mb_motors_get_yaw(mb_motors_const_t motors) { return motors->_yaw_in; }
static inline float mb_motors_get_yaw_ff(mb_motors_const_t motors) { return motors->_yaw_in_ff; }
static inline float mb_motors_get_throttle_out(mb_motors_const_t motors) { return motors->_throttle_out; }
static inline float mb_motors_get_throttle(mb_motors_const_t motors) { return math_constrain_float(lpf_get_output(&motors->_throttle_filter), 0.0f, 1.0f); }
static inline float mb_motors_get_throttle_bidirectional(mb_motors_const_t motors) { return math_constrain_float(2 * (lpf_get_output(&motors->_throttle_filter) - 0.5f), -1.0f, 1.0f); }
static inline float mb_motors_get_throttle_slew_rate(mb_motors_const_t motors) { return motors->_throttle_slew_rate; }
static inline float mb_motors_get_forward(mb_motors_const_t motors) { return motors->_forward_in; }
static inline float mb_motors_get_lateral(mb_motors_const_t motors) { return motors->_lateral_in; }

// motor failure handling
static inline void  mb_motors_set_thrust_boost(mb_motors_t motors, bool enable) { motors->_thrust_boost = enable; }
static inline bool  mb_motors_get_thrust_boost(mb_motors_const_t motors) { return motors->_thrust_boost; }

void                mb_motors_set_desired_spool_state(mb_motors_t motors, enum MotorDesiredSpoolState spool);
static inline enum MotorDesiredSpoolState mb_motors_get_desired_spool_state(mb_motors_const_t motors) { return motors->_spool_desired; }

// get_spool_state - get current spool state
static inline enum MotorSpoolState mb_motors_get_spool_state(mb_motors_const_t motors) { return motors->_spool_state; }

// set_density_ratio - sets air density as a proportion of sea level density
static inline void  mb_motors_set_air_density_ratio(mb_motors_t motors, float ratio) { motors->_air_density_ratio = ratio; }

// set_dt / get_dt - dt is the time since the last time the motor mixers were updated
//   _dt should be set based on the time of the last IMU read used by these controllers
//   the motor mixers should run on each loop to ensure normal operation
static inline void  mb_motors_set_dt(mb_motors_t motors, float dt) { motors->_dt = dt; }
static inline float mb_motors_get_dt(mb_motors_const_t motors) { return motors->_dt; }

// returns true if the configured PWM type is digital and should have fixed endpoints
bool                mb_motors_is_digital_pwm_type(mb_motors_const_t motors);

// returns true is pwm type is brushed
static inline bool  mb_motors_is_brushed_pwm_type(mb_motors_const_t motors) { return motors->_pwm_type == MB_MOTOR_PWM_TYPE_BRUSHED; }

// returns true is pwm type is normal
static inline bool  mb_motors_is_normal_pwm_type(mb_motors_const_t motors) { return (motors->_pwm_type == MB_MOTOR_PWM_TYPE_NORMAL) || (motors->_pwm_type == MB_MOTOR_PWM_TYPE_PWM_RANGE); }

static inline MAV_TYPE mb_motors_get_frame_mav_type(mb_motors_const_t motors) { return motors->_mav_type; }

/// 
bool mb_motors_ArmingChecks(mb_motors_const_t motors, size_t buflen, char *buffer);
void mb_motors_RcWrite(mb_motors_t motors, uint8_t chan, uint16_t pwm);
void mb_motors_RcWriteAngle(mb_motors_t motors, uint8_t chan, int16_t angle_cd);
void mb_motors_RcSetFreq(mb_motors_t motors, uint32_t motor_mask, uint16_t freq_hz);


/// ops

// check initialisation succeeded
static inline bool mb_motors_arming_checks(mb_motors_const_t motors, size_t buflen, char *buffer) {
    if (motors->ops->arming_checks) {
        return motors->ops->arming_checks(motors, buflen, buffer);
    }

    return mb_motors_ArmingChecks(motors, buflen, buffer);
}

// for 6DoF vehicles, sets the roll and pitch offset, this rotates the thrust vector in body frame
static inline void mb_motors_set_roll_pitch(mb_motors_t motors, float roll_deg, float pitch_deg) {
    if (motors->ops->set_roll_pitch) {
        motors->ops->set_roll_pitch(motors, roll_deg, pitch_deg);
    }
}

static inline float mb_motors_get_throttle_hover(mb_motors_const_t motors) {
    RT_ASSERT(motors->ops->get_throttle_hover != RT_NULL);
    return motors->ops->get_throttle_hover(motors);
}

// motor failure handling
static inline uint8_t mb_motors_get_lost_motor(mb_motors_const_t motors) {
    if (motors->ops->get_lost_motor) {
        return motors->ops->get_lost_motor(motors);
    }

    return 0;
}

// set update rate to motors - a value in hertz
static inline void mb_motors_set_update_rate(mb_motors_t motors, uint16_t speed_hz) {
    if (motors->ops->set_update_rate) {
        motors->ops->set_update_rate(motors, speed_hz);
        return;
    }

    motors->_speed_hz = speed_hz;
}

// init
static inline void mb_motors_init(mb_motors_t motors, enum mb_motor_frame_class frame_class, enum mb_motor_frame_type frame_type) {
    RT_ASSERT(motors->ops->init != RT_NULL);
    motors->ops->init(motors, frame_class, frame_type);
}

// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
static inline void mb_motors_set_frame_class_and_type(mb_motors_t motors, enum mb_motor_frame_class frame_class, enum mb_motor_frame_type frame_type) {
    RT_ASSERT(motors->ops->set_frame_class_and_type != RT_NULL);
    motors->ops->set_frame_class_and_type(motors, frame_class, frame_type);
}

// output - sends commands to the motors
static inline void mb_motors_output(mb_motors_t motors) {
    RT_ASSERT(motors->ops->output != RT_NULL);
    motors->ops->output(motors);
}

// output_min - sends minimum values out to the motors
static inline void mb_motors_output_min(mb_motors_t motors) {
    RT_ASSERT(motors->ops->output_min != RT_NULL);
    motors->ops->output_min(motors);
}

// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
//  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
static inline uint32_t mb_motors_get_motor_mask(mb_motors_t motors) {
    RT_ASSERT(motors->ops->get_motor_mask != RT_NULL);
    return motors->ops->get_motor_mask(motors);
}

// return the roll factor of any motor, this is used for tilt rotors and tail sitters
// using copter motors for forward flight
static inline float mb_motors_get_roll_factor(mb_motors_t motors, uint8_t i) {
    if (motors->ops->get_roll_factor) {
        return motors->ops->get_roll_factor(motors, i);
    }

    return 0.0f;
}

// return the pitch factor of any motor
static inline float mb_motors_get_pitch_factor(mb_motors_t motors, uint8_t i) {
    if (motors->ops->get_pitch_factor) {
        return motors->ops->get_pitch_factor(motors, i);
    }

    return 0.0f;
}

// return whether a motor is enabled or not
static inline bool mb_motors_is_motor_enabled(mb_motors_t motors, uint8_t i) {
    if (motors->ops->is_motor_enabled) {
        return motors->ops->is_motor_enabled(motors, i);
    }

    return false;
}

// This function required for tradheli. Tradheli initializes targets when going from unarmed to armed state.
// This function is overriden in motors_heli class.   Always true for multicopters.
static inline bool mb_motors_init_targets_on_arming(mb_motors_const_t motors) {
    if (motors->ops->init_targets_on_arming) {
        return motors->ops->init_targets_on_arming(motors);
    }

    return true;
}

// direct motor write
static inline void mb_motors_rc_write(mb_motors_t motors, uint8_t chan, uint16_t pwm) {
    if (motors->ops->rc_write) {
        motors->ops->rc_write(motors, chan, pwm);
        return;
    }

    mb_motors_RcWrite(motors, chan, pwm);
}

// write log, to be called at 10hz
static inline void mb_motors_Log_Write(mb_motors_t motors) {
    if (motors->ops->Log_Write) {
        motors->ops->Log_Write(motors);
    }
}

// output functions that should be overloaded by child classes
static inline void mb_motors_output_armed_stabilizing(mb_motors_t motors) {
    RT_ASSERT(motors->ops->output_armed_stabilizing != RT_NULL);
    motors->ops->output_armed_stabilizing(motors);
}

static inline void mb_motors_rc_write_angle(mb_motors_t motors, uint8_t chan, int16_t angle_cd) {
    if (motors->ops->rc_write_angle) {
        motors->ops->rc_write_angle(motors, chan, angle_cd);
        return;
    }

    mb_motors_RcWriteAngle(motors, chan, angle_cd);
}

static inline void mb_motors_rc_set_freq(mb_motors_t motors, uint32_t mask, uint16_t freq_hz) {
    if (motors->ops->rc_set_freq) {
        motors->ops->rc_set_freq(motors, mask, freq_hz);
        return;
    }

    mb_motors_RcSetFreq(motors, mask, freq_hz);
}

// update the throttle input filter
static inline void mb_motors_update_throttle_filter(mb_motors_t motors) {
    RT_ASSERT(motors->ops->update_throttle_filter != RT_NULL);
    motors->ops->update_throttle_filter(motors);
}

// save parameters as part of disarming
static inline void mb_motors_save_params_on_disarm(mb_motors_t motors) {
    if (motors->ops->save_params_on_disarm) {
        motors->ops->save_params_on_disarm(motors);
    }
}

// return string corresponding to frame_class
static inline const char* mb_motors_get_frame_string(mb_motors_const_t motors) {
    RT_ASSERT(motors->ops->_get_frame_string != RT_NULL);
    return motors->ops->_get_frame_string(motors);
}

// return string corresponding to frame_type
static inline const char* mb_motors_get_type_string(mb_motors_const_t motors) {
    if (motors->ops->get_type_string) {
        return motors->ops->get_type_string(motors);
    }

    return "";
}

// set the value of motor module parameters
static inline void  mb_motors_assign_param(mb_motors_t motors) {
    RT_ASSERT(motors->ops->assign_param != RT_NULL);
    motors->ops->assign_param(motors);
}
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



